#include "plot.h"
#include <cmath>

PLOT::PLOT()
{

}

void PLOT::DrawMap(QPainter *painter)
{
    sim.Initialize();
    painter->drawImage(0,0,*(sim.map.mapImage));
}

void PLOT::DrawCameraScan(QPainter *painter)
{
    int count = 0;
    painter->save();
    painter->translate(400,100);

    for(int i=sim.car.camera.height-1;i>=0;i--)
    {
        for(int j=0;j<sim.car.camera.width;j++)
        {
            painter->setPen(this->sim.car.camera.scanImage[count++]);
            painter->drawPoint(j,i);
        }
    }
    painter->restore();
}

void PLOT::DrawCar(QPainter *painter)
{
    double x=sim.car.posX;
    double y=sim.car.posY;
    double theata=sim.car.posTheata;
    int viewHeight=sim.car.camera.height;
    int viewWidth =sim.car.camera.width;

    painter->setPen(QColor(255,0,0));
    painter->drawLine(x-viewWidth/2*cos(theata),y+viewWidth/2*sin(theata), //point1
                      x+viewWidth/2*cos(theata),y-viewWidth/2*sin(theata));//point2
    painter->drawLine(x-viewWidth/2*cos(theata)-viewHeight*sin(theata),
                      y+viewWidth/2*sin(theata)-viewHeight*cos(theata),   //point3
                      x+viewWidth/2*cos(theata)-viewHeight*sin(theata),
                      y-viewWidth/2*sin(theata)-viewHeight*cos(theata));  //point4

    painter->drawLine(x-viewWidth/2*cos(theata),y+viewWidth/2*sin(theata),
                      x-viewWidth/2*cos(theata)-viewHeight*sin(theata),
                      y+viewWidth/2*sin(theata)-viewHeight*cos(theata));
    painter->drawLine(x+viewWidth/2*cos(theata),y-viewWidth/2*sin(theata),
                      x+viewWidth/2*cos(theata)-viewHeight*sin(theata),
                      y-viewWidth/2*sin(theata)-viewHeight*cos(theata));
    painter->drawLine(x+5*sin(theata),y+5*cos(theata),x-viewHeight*sin(theata),y-viewHeight*cos(theata));
    painter->drawPoint(x,y);
}
